//#include <pcl/console/print.h>
#include "PathCalculator\PathCalculator.h"
//#include "GlutDemoApplication.h"
//#include "LinearMath/btAlignedObjectArray.h"
#include <stdio.h>
#include <iostream>
#include "KinematicModel\Point.h"
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/io/openni_grabber.h>
#include <pcl/visualization/cloud_viewer.h>
#include "OpenNIScreenCapturer.h"
#include "boost/filesystem.hpp"


int captureScreen();
int getMainCluster();
std::vector< object > nearestNeighbors(int cluster_no);



vector<Point> classifier(int movementClass, OpenNIScreenCapturer* o){

	int no;
	std::vector< object > objects;
	stringstream ss;
	std::vector<std::string> cloud_path;
	std::vector<Point> path;

    pcl::console::print_info ("Capturing camera data... \n"); 
	
	//take a screenshot
	//captureScreen();
	
	
	//remove("scene.pcd");
	//o->save_one = true;
	std::cout << "Waiting..";
	while(false /*o->save_one*/)   //Se si usa Kinect cambiare false con o->save_one
	{ 
		std::cout << "."; 
		Sleep(200); 
	}


	//clusters it
	int x = getMainCluster();
	//find NN
	objects = nearestNeighbors(x);

	std::cout << "Objects size: " << objects.size() << std::endl;

	if(objects.size() > 0){
		
		printf("\n\n\nInsert the number of the cluster representing the object you would like to pick up: ");
		scanf("%d",&no);	
  
		if( no + 1 > objects.size() ){
			std::cout << "Wrong cluster index" << std::endl;
			return path; 
		}
		int i;
		for(i = 0; i < objects.size(); i++){
		
			ss << "cloud_cluster_" << objects.at(i).clusterNo << ".pcd";
			cloud_path.push_back(ss.str());
			ss.str("");

		}
	
		PathCalculator PC(cloud_path, 0.005, objects, no, movementClass);
		path = PC.getPath();


		std::cout << "CLASS: " << path.size() << std::endl;
	}
	else{
		PathCalculator PC(movementClass);
		path = PC.getPath();
	}


	pcl::console::print_info ("\nDone!");
	/*char key;
	std::cin >> key;*/
	return path;
}